#include <iostream>
#include "igstkPivotCalibrationAlgorithm.h"
#include "igstkCoordinateSystemTransformToResult.h"


/** Class for observing the invalid request error.*/
class InvalidRequestErrorEventObserver : public itk::Command
{
public:
  typedef InvalidRequestErrorEventObserver            Self;
  typedef itk::SmartPointer<Self>                     Pointer;
  typedef itk::Command                                Superclass;
  itkNewMacro(Self);

void Execute(const itk::Object *caller, const itk::EventObject & event)
{
  const itk::Object * constCaller = caller;
  this->Execute(constCaller, event);
}

void Execute(itk::Object * itkNotUsed(caller), const itk::EventObject & itkNotUsed(event))
{
  std::cout<<"Invalid method request, cannot invoke this method in current ";
  std::cout<<"object state."<<std::endl;
  this->m_EventOccured = true;
}

bool EventOccured()
{
  return this->m_EventOccured;
}

void Reset()
{
  this->m_EventOccured = false;
}

protected:
  InvalidRequestErrorEventObserver() : m_EventOccured(false) {}
  virtual ~InvalidRequestErrorEventObserver(){}

private:
  InvalidRequestErrorEventObserver(const Self&);
  void operator=(const Self&); 

  bool m_EventOccured;
};

/** Class for observing events that carry a payload. The user can query the 
 *  class if an event occured (EventOccured), access the payload (Get()), and 
 *  reset (EventOccured==false) the observer (Reset()).*/
template <class EventType>
class PayloadEventObserver : public itk::Command
{
public:
  typedef PayloadEventObserver            Self;
  typedef itk::SmartPointer<Self>         Pointer;
  typedef itk::Command                    Superclass;
  itkNewMacro(Self);

  void Execute(const itk::Object *caller, const itk::EventObject & event)
  {
    const itk::Object * constCaller = caller;
    this->Execute(constCaller, event);
  }

  void Execute(itk::Object * itkNotUsed(caller), const itk::EventObject & event)
  {
    const EventType *evt = dynamic_cast< const EventType * > (&event);
    if(evt) 
    {
      this->m_EventOccured = true;
      this->m_Payload = evt->Get();
    }
  }

  typename EventType::PayloadType Get() 
  {
    return this->m_Payload;
  }
  
  bool EventOccured()
  {
    return this->m_EventOccured;
  }

  void Reset()
  {
    this->m_EventOccured = false;
  }

protected:
  PayloadEventObserver() : m_EventOccured(false) {}
  virtual ~PayloadEventObserver(){}

private:
  PayloadEventObserver(const Self&);
  void operator=(const Self&); 

  bool m_EventOccured;
  typename EventType::PayloadType m_Payload;
};


typedef PayloadEventObserver< igstk::CoordinateSystemTransformToEvent > 
  TransformEventObserver;
typedef PayloadEventObserver< igstk::PointEvent > PivotPointEventObserver;
typedef PayloadEventObserver< igstk::DoubleTypeEvent > RMSEEventObserver;


class CalibrationEventObserver : public itk::Command
{
public:
  typedef CalibrationEventObserver            Self;
  typedef itk::SmartPointer<Self>             Pointer;
  typedef itk::Command                        Superclass;
  itkNewMacro(Self);

  void Execute(const itk::Object *caller, const itk::EventObject & event)
  {
    const itk::Object * constCaller = caller;
    this->Execute(constCaller, event);
  }

  void Execute(itk::Object * itkNotUsed(caller), const itk::EventObject & event)
  {
    if( dynamic_cast< const igstk::PivotCalibrationAlgorithm::CalibrationSuccessEvent * > (&event) ) 
    {
      std::cout<<"Calibration succeeded."<<std::endl;
      this->m_Success = true;
    }
    else if( dynamic_cast< const igstk::PivotCalibrationAlgorithm::CalibrationFailureEvent * > (&event) ) 
    {
      std::cout<<"Calibration failed."<<std::endl;
      this->m_Failure = true;
    }
  }

  bool SuccessEventOccured()
  {
    return this->m_Success;
  }

  bool FailureEventOccured()
  {
    return this->m_Failure;
  }

  void Reset()
  {
    this->m_Success = false;
    this->m_Failure = false;
  }

protected:
  CalibrationEventObserver () : m_Success(false), m_Failure(false) {};
  virtual ~CalibrationEventObserver (){}

private:
  CalibrationEventObserver (const Self&);
  void operator=(const Self&); 

  bool m_Success;
  bool m_Failure;
};

                                //15 second acquisition at 40Hz
static const unsigned int NUMBER_OF_VALID_TRANSFORMATIONS = 600; 
static double pivotCalibrationValidDataSet[NUMBER_OF_VALID_TRANSFORMATIONS][7] =
{
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.4, -8.59, -205.45, -0.011301, -0.691185, 0, 0.722589},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.44, -8.63, -205.52, -0.011801, -0.69154, 0, 0.722242},
{90.44, -8.63, -205.52, -0.011801, -0.69154, 0, 0.722242},
{90.42, -8.63, -205.51, -0.012101, -0.691742, 0, 0.722044},
{90.42, -8.63, -205.51, -0.012101, -0.691742, 0, 0.722044},
{90.46, -8.62, -205.52, -0.012501, -0.692045, 0, 0.721747},
{90.46, -8.62, -205.52, -0.012501, -0.692045, 0, 0.721747},
{90.44, -8.6, -205.53, -0.012901, -0.692347, 0, 0.721449},
{90.44, -8.6, -205.53, -0.012901, -0.692347, 0, 0.721449},
{90.49, -8.6, -205.57, -0.013401, -0.692853, 0, 0.720955},
{90.49, -8.6, -205.57, -0.013401, -0.692853, 0, 0.720955},
{90.49, -8.59, -205.6, -0.013901, -0.693357, 0, 0.72046},
{90.49, -8.59, -205.6, -0.013901, -0.693357, 0, 0.72046},
{90.54, -8.63, -205.67, -0.0146, -0.694115, 0, 0.719716},
{90.54, -8.63, -205.67, -0.0146, -0.694115, 0, 0.719716},
{90.53, -8.62, -205.71, -0.015302, -0.69487, 0, 0.718972},
{90.53, -8.62, -205.71, -0.015302, -0.69487, 0, 0.718972},
{90.53, -8.62, -205.71, -0.015302, -0.69487, 0, 0.718972},
{90.61, -8.67, -205.8, -0.015901, -0.695931, 0, 0.717932},
{90.63, -8.66, -205.85, -0.016101, -0.696944, 0, 0.716945},
{90.63, -8.66, -205.85, -0.016101, -0.696944, 0, 0.716945},
{90.72, -8.71, -205.94, -0.016501, -0.698357, 0, 0.715559},
{90.72, -8.71, -205.94, -0.016501, -0.698357, 0, 0.715559},
{90.79, -8.73, -206.08, -0.016602, -0.699873, 0, 0.714074},
{90.79, -8.73, -206.08, -0.016602, -0.699873, 0, 0.714074},
{90.9, -8.83, -206.25, -0.017201, -0.701833, 0, 0.712134},
{90.9, -8.83, -206.25, -0.017201, -0.701833, 0, 0.712134},
{90.99, -8.88, -206.43, -0.017701, -0.70414, 0, 0.70984},
{90.99, -8.88, -206.43, -0.017701, -0.70414, 0, 0.70984},
{91.14, -8.97, -206.65, -0.018801, -0.706932, 0, 0.707032},
{91.14, -8.97, -206.65, -0.018801, -0.706932, 0, 0.707032},
{91.14, -8.97, -206.65, -0.018801, -0.706932, 0, 0.707032},
{90.26, -8.4, -206.46, -0.023801, -0.724434, 0, 0.688933},
{90.26, -8.4, -206.46, -0.023801, -0.724434, 0, 0.688933},
{90.3, -8.29, -206.51, -0.028202, -0.730854, 0, 0.681951},
{90.3, -8.29, -206.51, -0.028202, -0.730854, 0, 0.681951},
{90.13, -8.32, -206.83, -0.031202, -0.737658, 0, 0.674453},
{90.13, -8.32, -206.83, -0.031202, -0.737658, 0, 0.674453},
{90.16, -8.38, -207.04, -0.034901, -0.745025, 0, 0.666123},
{90.16, -8.38, -207.04, -0.034901, -0.745025, 0, 0.666123},
{90.22, -8.34, -207.44, -0.036302, -0.753151, 0, 0.656845},
{90.22, -8.34, -207.44, -0.036302, -0.753151, 0, 0.656845},
{90.21, -8.32, -207.62, -0.038503, -0.760456, 0, 0.648247},
{90.21, -8.32, -207.62, -0.038503, -0.760456, 0, 0.648247},
{90.26, -8.24, -207.89, -0.039502, -0.766243, 0, 0.641336},
{90.26, -8.24, -207.89, -0.039502, -0.766243, 0, 0.641336},
{90.12, -8.2, -207.51, -0.040804, -0.770068, 0, 0.636656},
{90.12, -8.2, -207.51, -0.040804, -0.770068, 0, 0.636656},
{90.33, -8.19, -207.88, -0.042402, -0.773537, 0, 0.632331},
{90.33, -8.19, -207.88, -0.042402, -0.773537, 0, 0.632331},
{90.27, -8.25, -207.84, -0.042904, -0.776173, 0, 0.629059},
{90.27, -8.25, -207.84, -0.042904, -0.776173, 0, 0.629059},
{90.37, -8.22, -208.14, -0.040904, -0.779669, 0, 0.624855},
{90.37, -8.22, -208.14, -0.040904, -0.779669, 0, 0.624855},
{90.24, -8.25, -207.87, -0.037404, -0.784287, 0, 0.619269},
{90.24, -8.25, -207.87, -0.037404, -0.784287, 0, 0.619269},
{90.33, -8.35, -208.4, -0.033202, -0.790052, 0, 0.61214},
{90.33, -8.35, -208.4, -0.033202, -0.790052, 0, 0.61214},
{90.28, -8.42, -208.11, -0.031301, -0.795134, 0, 0.605626},
{90.28, -8.42, -208.11, -0.031301, -0.795134, 0, 0.605626},
{90.28, -8.42, -208.11, -0.031301, -0.795134, 0, 0.605626},
{90.17, -8.35, -208.39, -0.032601, -0.804017, 0, 0.593712},
{90.17, -8.35, -208.39, -0.032601, -0.804017, 0, 0.593712},
{90.37, -8.29, -208.89, -0.033903, -0.808474, 0, 0.587554},
{90.37, -8.29, -208.89, -0.033903, -0.808474, 0, 0.587554},
{90.17, -8.32, -208.5, -0.034302, -0.812152, 0, 0.582437},
{90.17, -8.32, -208.5, -0.034302, -0.812152, 0, 0.582437},
{90.17, -8.32, -208.5, -0.034302, -0.812152, 0, 0.582437},
{90.51, -8.37, -209.31, -0.034203, -0.815569, 0, 0.577649},
{90.23, -8.3, -208.52, -0.033604, -0.817697, 0, 0.574668},
{90.23, -8.3, -208.52, -0.033604, -0.817697, 0, 0.574668},
{90.51, -8.39, -209.34, -0.033101, -0.819923, 0, 0.571516},
{90.51, -8.39, -209.34, -0.033101, -0.819923, 0, 0.571516},
{90.27, -8.36, -208.66, -0.031803, -0.821888, 0, 0.568761},
{90.27, -8.36, -208.66, -0.031803, -0.821888, 0, 0.568761},
{90.5, -8.39, -209.38, -0.030802, -0.82415, 0, 0.565534},
{90.5, -8.39, -209.38, -0.030802, -0.82415, 0, 0.565534},
{90.38, -8.33, -208.73, -0.030603, -0.82647, 0, 0.562148},
{90.38, -8.33, -208.73, -0.030603, -0.82647, 0, 0.562148},
{91.73, -8.88, -210.53, -0.034402, -0.820552, 0, 0.570536},
{91.73, -8.88, -210.53, -0.034402, -0.820552, 0, 0.570536},
{91.72, -8.84, -210.56, -0.034201, -0.823414, 0, 0.56641},
{91.72, -8.84, -210.56, -0.034201, -0.823414, 0, 0.56641},
{91.79, -8.86, -210.73, -0.034103, -0.826267, 0, 0.562246},
{91.79, -8.86, -210.73, -0.034103, -0.826267, 0, 0.562246},
{90.33, -8.18, -209.07, -0.035603, -0.840879, 0, 0.540051},
{90.33, -8.18, -209.07, -0.035603, -0.840879, 0, 0.540051},
{90.83, -8.3, -209.77, -0.040502, -0.845132, 0, 0.53302},
{90.83, -8.3, -209.77, -0.040502, -0.845132, 0, 0.53302},
{90.48, -8.11, -209.32, -0.045904, -0.848472, 0, 0.527245},
{90.48, -8.11, -209.32, -0.045904, -0.848472, 0, 0.527245},
{90.94, -8.19, -209.98, -0.052004, -0.852372, 0, 0.520344},
{90.94, -8.19, -209.98, -0.052004, -0.852372, 0, 0.520344},
{90.58, -8.08, -209.39, -0.057006, -0.855383, 0, 0.51485},
{90.58, -8.08, -209.39, -0.057006, -0.855383, 0, 0.51485},
{90.96, -8.09, -209.93, -0.063401, -0.858718, 0, 0.508511},
{90.96, -8.09, -209.93, -0.063401, -0.858718, 0, 0.508511},
{90.76, -7.94, -209.52, -0.067105, -0.862064, 0, 0.502337},
{90.76, -7.94, -209.52, -0.067105, -0.862064, 0, 0.502337},
{91.1, -7.97, -210.04, -0.071907, -0.865083, 0, 0.496448},
{91.1, -7.97, -210.04, -0.071907, -0.865083, 0, 0.496448},
{90.85, -7.89, -209.91, -0.072806, -0.867467, 0, 0.492138},
{90.85, -7.89, -209.91, -0.072806, -0.867467, 0, 0.492138},
{91.15, -7.96, -210.05, -0.075005, -0.870157, 0, 0.487032},
{91.15, -7.96, -210.05, -0.075005, -0.870157, 0, 0.487032},
{90.96, -7.83, -209.82, -0.075705, -0.872656, 0, 0.482431},
{90.96, -7.83, -209.82, -0.075705, -0.872656, 0, 0.482431},
{90.96, -7.83, -209.82, -0.075705, -0.872656, 0, 0.482431},
{91.27, -8, -210.26, -0.076706, -0.875366, 0, 0.477336},
{90.98, -7.86, -210.21, -0.075606, -0.877469, 0, 0.473637},
{90.98, -7.86, -210.21, -0.075606, -0.877469, 0, 0.473637},
{91.25, -8.01, -210.1, -0.076406, -0.879973, 0, 0.468839},
{91.25, -8.01, -210.1, -0.076406, -0.879973, 0, 0.468839},
{91.22, -7.79, -210.22, -0.079604, -0.88224, 0, 0.464021},
{91.22, -7.79, -210.22, -0.079604, -0.88224, 0, 0.464021},
{91.44, -7.9, -210.1, -0.083505, -0.884852, 0, 0.458327},
{91.44, -7.9, -210.1, -0.083505, -0.884852, 0, 0.458327},
{91.18, -7.85, -210.38, -0.085904, -0.886942, 0, 0.453822},
{91.18, -7.85, -210.38, -0.085904, -0.886942, 0, 0.453822},
{91.56, -7.81, -210.31, -0.092809, -0.889685, 0, 0.447043},
{91.56, -7.81, -210.31, -0.092809, -0.889685, 0, 0.447043},
{91.44, -7.71, -210.85, -0.096708, -0.891571, 0, 0.442435},
{91.44, -7.71, -210.85, -0.096708, -0.891571, 0, 0.442435},
{91.71, -7.81, -210.38, -0.100602, -0.893618, 0, 0.437409},
{91.71, -7.81, -210.38, -0.100602, -0.893618, 0, 0.437409},
{91.5, -7.65, -210.66, -0.103311, -0.894996, 0, 0.433946},
{91.5, -7.65, -210.66, -0.103311, -0.894996, 0, 0.433946},
{91.74, -7.72, -210.14, -0.109409, -0.896972, 0, 0.428335},
{91.74, -7.72, -210.14, -0.109409, -0.896972, 0, 0.428335},
{91.63, -7.58, -211.01, -0.113207, -0.898357, 0, 0.424427},
{91.63, -7.58, -211.01, -0.113207, -0.898357, 0, 0.424427},
{91.88, -7.67, -210.43, -0.116502, -0.900617, 0, 0.418708},
{91.88, -7.67, -210.43, -0.116502, -0.900617, 0, 0.418708},
{91.91, -7.51, -211, -0.118306, -0.901948, 0, 0.415322},
{91.91, -7.51, -211, -0.118306, -0.901948, 0, 0.415322},
{92.79, -8.04, -212.2, -0.107605, -0.897244, 0, 0.428221},
{92.79, -8.04, -212.2, -0.107605, -0.897244, 0, 0.428221},
{92.73, -7.96, -211.99, -0.110806, -0.898752, 0, 0.424225},
{92.73, -7.96, -211.99, -0.110806, -0.898752, 0, 0.424225},
{92.67, -7.92, -211.89, -0.113206, -0.900146, 0, 0.420621},
{92.64, -7.88, -211.78, -0.115105, -0.901441, 0, 0.417319},
{92.64, -7.88, -211.78, -0.115105, -0.901441, 0, 0.417319},
{92.56, -7.84, -211.66, -0.11661, -0.902577, 0, 0.414435},
{92.56, -7.84, -211.66, -0.11661, -0.902577, 0, 0.414435},
{92.5, -7.79, -211.48, -0.117612, -0.903589, 0, 0.41194},
{92.5, -7.79, -211.48, -0.117612, -0.903589, 0, 0.41194},
{92.41, -7.75, -211.31, -0.117904, -0.904427, 0, 0.410012},
{92.41, -7.75, -211.31, -0.117904, -0.904427, 0, 0.410012},
{92.28, -7.7, -211.08, -0.117913, -0.905, 0, 0.408745},
{92.28, -7.7, -211.08, -0.117913, -0.905, 0, 0.408745},
{92.08, -7.65, -210.89, -0.117714, -0.905205, 0, 0.408347},
{92.08, -7.65, -210.89, -0.117714, -0.905205, 0, 0.408347},
{92.08, -7.65, -210.89, -0.117714, -0.905205, 0, 0.408347},
{91.83, -7.58, -210.54, -0.117404, -0.905034, 0, 0.408815},
{91.59, -7.53, -210.28, -0.116513, -0.904501, 0, 0.410246},
{91.59, -7.53, -210.28, -0.116513, -0.904501, 0, 0.410246},
{91.34, -7.49, -209.98, -0.115408, -0.90356, 0, 0.412627},
{91.34, -7.49, -209.98, -0.115408, -0.90356, 0, 0.412627},
{91.06, -7.42, -209.66, -0.114211, -0.902191, 0, 0.415942},
{91.06, -7.42, -209.66, -0.114211, -0.902191, 0, 0.415942},
{91.06, -7.42, -209.66, -0.114211, -0.902191, 0, 0.415942},
{91.58, -7.55, -210.99, -0.109614, -0.890016, 0, 0.442557},
{91.58, -7.55, -210.99, -0.109614, -0.890016, 0, 0.442557},
{91.51, -7.68, -209.8, -0.10781, -0.88668, 0, 0.44964},
{91.51, -7.68, -209.8, -0.10781, -0.88668, 0, 0.44964},
{91.45, -7.61, -210.82, -0.109806, -0.881247, 0, 0.459724},
{91.45, -7.61, -210.82, -0.109806, -0.881247, 0, 0.459724},
{91.14, -7.66, -209.59, -0.109409, -0.876374, 0, 0.46904},
{91.14, -7.66, -209.59, -0.109409, -0.876374, 0, 0.46904},
{91.28, -7.51, -210.64, -0.11301, -0.870377, 0, 0.479242},
{91.28, -7.51, -210.64, -0.11301, -0.870377, 0, 0.479242},
{90.95, -7.61, -209.34, -0.112205, -0.864741, 0, 0.489523},
{90.95, -7.61, -209.34, -0.112205, -0.864741, 0, 0.489523},
{91.14, -7.49, -210.49, -0.112911, -0.857787, 0, 0.501451},
{91.14, -7.49, -210.49, -0.112911, -0.857787, 0, 0.501451},
{90.64, -7.74, -209.37, -0.107206, -0.851047, 0, 0.514029},
{90.64, -7.74, -209.37, -0.107206, -0.851047, 0, 0.514029},
{90.97, -7.56, -210.03, -0.106902, -0.843919, 0, 0.525712},
{90.97, -7.56, -210.03, -0.106902, -0.843919, 0, 0.525712},
{90.56, -7.76, -209.11, -0.102506, -0.837053, 0, 0.537434},
{90.56, -7.76, -209.11, -0.102506, -0.837053, 0, 0.537434},
{90.76, -7.62, -209.88, -0.101005, -0.829943, 0, 0.548628},
{90.76, -7.62, -209.88, -0.101005, -0.829943, 0, 0.548628},
{90.31, -7.79, -208.58, -0.096006, -0.822954, 0, 0.559937},
{90.31, -7.79, -208.58, -0.096006, -0.822954, 0, 0.559937},
{90.65, -7.5, -209.35, -0.098111, -0.815791, 0, 0.569964},
{90.65, -7.5, -209.35, -0.098111, -0.815791, 0, 0.569964},
{90.25, -7.78, -208.66, -0.095807, -0.808359, 0, 0.580842},
{90.25, -7.78, -208.66, -0.095807, -0.808359, 0, 0.580842},
{90.54, -7.66, -209.03, -0.09551, -0.800286, 0, 0.591963},
{90.54, -7.66, -209.03, -0.09551, -0.800286, 0, 0.591963},
{90.15, -7.83, -208.1, -0.091908, -0.791472, 0, 0.604255},
{90.15, -7.83, -208.1, -0.091908, -0.791472, 0, 0.604255},
{90.47, -7.55, -208.39, -0.094306, -0.782446, 0, 0.615536},
{90.47, -7.55, -208.39, -0.094306, -0.782446, 0, 0.615536},
{90.29, -7.8, -207.8, -0.092109, -0.773179, 0, 0.627464},
{90.29, -7.8, -207.8, -0.092109, -0.773179, 0, 0.627464},
{90.49, -7.61, -207.98, -0.09171, -0.76358, 0, 0.639167},
{90.49, -7.61, -207.98, -0.09171, -0.76358, 0, 0.639167},
{90.21, -7.79, -207.33, -0.089911, -0.753492, 0, 0.65128},
{90.21, -7.79, -207.33, -0.089911, -0.753492, 0, 0.65128},
{90.21, -7.79, -207.33, -0.089911, -0.753492, 0, 0.65128},
{90.47, -7.59, -207.52, -0.091307, -0.743255, 0, 0.662749},
{90.29, -7.61, -206.77, -0.08721, -0.732483, 0, 0.675176},
{90.29, -7.61, -206.77, -0.08721, -0.732483, 0, 0.675176},
{90.51, -7.59, -206.66, -0.082606, -0.720949, 0, 0.688047},
{90.51, -7.59, -206.66, -0.082606, -0.720949, 0, 0.688047},
{90.39, -7.8, -206.05, -0.075205, -0.708247, 0, 0.701947},
{90.39, -7.8, -206.05, -0.075205, -0.708247, 0, 0.701947},
{90.55, -7.86, -206, -0.069606, -0.694863, 0, 0.715765},
{90.55, -7.86, -206, -0.069606, -0.694863, 0, 0.715765},
{90.48, -7.73, -205.02, -0.060003, -0.68163, 0, 0.729232},
{90.48, -7.73, -205.02, -0.060003, -0.68163, 0, 0.729232},
{90.73, -8.48, -205.06, -0.054903, -0.667541, 0, 0.742546},
{90.73, -8.48, -205.06, -0.054903, -0.667541, 0, 0.742546},
{90.75, -7.95, -204.35, -0.043301, -0.653814, 0, 0.755416},
{90.75, -7.95, -204.35, -0.043301, -0.653814, 0, 0.755416},
{90.71, -8.31, -204.13, -0.035605, -0.640182, 0, 0.767398},
{90.71, -8.31, -204.13, -0.035605, -0.640182, 0, 0.767398},
{90.9, -8.14, -203.54, -0.026102, -0.628146, 0, 0.777657},
{90.9, -8.14, -203.54, -0.026102, -0.628146, 0, 0.777657},
{90.96, -8.5, -203.49, -0.022803, -0.615269, 0, 0.787988},
{90.96, -8.5, -203.49, -0.022803, -0.615269, 0, 0.787988},
{91.1, -8.22, -202.9, -0.016501, -0.60372, 0, 0.797026},
{91.1, -8.22, -202.9, -0.016501, -0.60372, 0, 0.797026},
{91.07, -8.65, -202.74, -0.014101, -0.592055, 0, 0.805774},
{91.07, -8.65, -202.74, -0.014101, -0.592055, 0, 0.805774},
{91.33, -8.25, -202.22, -0.0084, -0.582834, 0, 0.812548},
{91.33, -8.25, -202.22, -0.0084, -0.582834, 0, 0.812548},
{91.34, -8.85, -202.37, -0.009601, -0.572751, 0, 0.819673},
{91.34, -8.85, -202.37, -0.009601, -0.572751, 0, 0.819673},
{91.66, -7.97, -201.74, -0.003, -0.564628, 0, 0.82534},
{91.66, -7.97, -201.74, -0.003, -0.564628, 0, 0.82534},
{91.48, -9.03, -201.8, -0.0044, -0.553933, 0, 0.83255},
{91.48, -9.03, -201.8, -0.0044, -0.553933, 0, 0.83255},
{91.83, -8.33, -201.28, 0.002, -0.545131, 0, 0.838348},
{91.83, -8.33, -201.28, 0.002, -0.545131, 0, 0.838348},
{91.69, -9, -201.36, 0.0007, -0.534531, 0, 0.845149},
{91.69, -9, -201.36, 0.0007, -0.534531, 0, 0.845149},
{92.35, -8.11, -200.62, 0.0074, -0.526826, 0, 0.849941},
{92.35, -8.11, -200.62, 0.0074, -0.526826, 0, 0.849941},
{92.09, -9.14, -200.76, 0.005, -0.513342, 0, 0.85817},
{92.09, -9.14, -200.76, 0.005, -0.513342, 0, 0.85817},
{92.45, -8.51, -200.32, 0.009801, -0.501856, 0, 0.864896},
{92.45, -8.51, -200.32, 0.009801, -0.501856, 0, 0.864896},
{92.35, -9.09, -200.16, 0.007401, -0.489844, 0, 0.871779},
{92.35, -9.09, -200.16, 0.007401, -0.489844, 0, 0.871779},
{92.81, -8.07, -199.7, 0.012001, -0.480624, 0, 0.876844},
{92.81, -8.07, -199.7, 0.012001, -0.480624, 0, 0.876844},
{92.84, -9.42, -199.85, 0.004101, -0.468959, 0, 0.883211},
{92.84, -9.42, -199.85, 0.004101, -0.468959, 0, 0.883211},
{93.23, -8.12, -199.32, 0.0111, -0.458908, 0, 0.888415},
{93.23, -8.12, -199.32, 0.0111, -0.458908, 0, 0.888415},
{92.93, -8.45, -199.28, 0.009801, -0.445751, 0, 0.895103},
{92.93, -8.45, -199.28, 0.009801, -0.445751, 0, 0.895103},
{92.93, -8.45, -199.28, 0.009801, -0.445751, 0, 0.895103},
{93.71, -7.88, -198.96, 0.012801, -0.436947, 0, 0.899396},
{92.92, -7.03, -198.3, 0.016601, -0.420028, 0, 0.90736},
{92.92, -7.03, -198.3, 0.016601, -0.420028, 0, 0.90736},
{93.84, -7.59, -198.14, 0.012001, -0.411225, 0, 0.911455},
{93.84, -7.59, -198.14, 0.012001, -0.411225, 0, 0.911455},
{93.34, -6.5, -197.88, 0.018501, -0.394914, 0, 0.918532},
{93.34, -6.5, -197.88, 0.018501, -0.394914, 0, 0.918532},
{94.37, -7.78, -197.66, 0.0091, -0.387012, 0, 0.92203},
{94.37, -7.78, -197.66, 0.0091, -0.387012, 0, 0.92203},
{93.79, -6.45, -197.46, 0.0163, -0.370203, 0, 0.928808},
{93.79, -6.45, -197.46, 0.0163, -0.370203, 0, 0.928808},
{94.63, -7.37, -197.3, 0.008001, -0.362343, 0, 0.93201},
{94.63, -7.37, -197.3, 0.008001, -0.362343, 0, 0.93201},
{94.44, -7.37, -197.22, 0.007001, -0.350633, 0, 0.936487},
{94.44, -7.37, -197.22, 0.007001, -0.350633, 0, 0.936487},
{95.03, -7.51, -196.93, 0.005801, -0.343335, 0, 0.939195},
{95.03, -7.51, -196.93, 0.005801, -0.343335, 0, 0.939195},
{94.49, -7, -196.82, 0.010901, -0.331231, 0, 0.943487},
{94.49, -7, -196.82, 0.010901, -0.331231, 0, 0.943487},
{95.26, -7.76, -196.71, 0.0058, -0.328915, 0, 0.944342},
{95.26, -7.76, -196.71, 0.0058, -0.328915, 0, 0.944342},
{95.05, -7.32, -196.61, 0.0096, -0.321608, 0, 0.946824},
{95.05, -7.32, -196.61, 0.0096, -0.321608, 0, 0.946824},
{95.2, -7.44, -196.58, 0.009101, -0.317025, 0, 0.948374},
{95.2, -7.44, -196.58, 0.009101, -0.317025, 0, 0.948374},
{95.11, -7.35, -196.53, 0.0097, -0.313211, 0, 0.949634},
{95.11, -7.35, -196.53, 0.0097, -0.313211, 0, 0.949634},
{95.57, -7.71, -196.44, 0.0054, -0.314122, 0, 0.949367},
{95.57, -7.71, -196.44, 0.0054, -0.314122, 0, 0.949367},
{95.3, -7.64, -196.46, 0.0046, -0.312021, 0, 0.950064},
{95.3, -7.64, -196.46, 0.0046, -0.312021, 0, 0.950064},
{95.56, -9.21, -196.23, -0.0035, -0.326318, 0, 0.945253},
{95.56, -9.21, -196.23, -0.0035, -0.326318, 0, 0.945253},
{95.59, -8.84, -196.24, -0.0013, -0.322101, 0, 0.946704},
{95.55, -8.45, -196.24, 0.0008, -0.318327, 0, 0.947981},
{95.55, -8.45, -196.24, 0.0008, -0.318327, 0, 0.947981},
{95.54, -8.15, -196.26, 0.0022, -0.315717, 0, 0.948851},
{95.54, -8.15, -196.26, 0.0022, -0.315717, 0, 0.948851},
{95.52, -7.91, -196.26, 0.003, -0.313735, 0, 0.949506},
{95.52, -7.91, -196.26, 0.003, -0.313735, 0, 0.949506},
{95.48, -7.65, -196.32, 0.0039, -0.312322, 0, 0.949968},
{95.48, -7.65, -196.32, 0.0039, -0.312322, 0, 0.949968},
{95.41, -7.32, -196.37, 0.0051, -0.31172, 0, 0.95016},
{95.41, -7.32, -196.37, 0.0051, -0.31172, 0, 0.95016},
{95.31, -7.01, -196.48, 0.0063, -0.311808, 0, 0.950124},
{95.31, -7.01, -196.48, 0.0063, -0.311808, 0, 0.950124},
{95.23, -6.72, -196.57, 0.0074, -0.312917, 0, 0.949752},
{95.23, -6.72, -196.57, 0.0074, -0.312917, 0, 0.949752},
{95.15, -6.37, -196.7, 0.0091, -0.314714, 0, 0.949143},
{95.15, -6.37, -196.7, 0.0091, -0.314714, 0, 0.949143},
{95.15, -6.37, -196.7, 0.0091, -0.314714, 0, 0.949143},
{94.85, -7.25, -197.01, 0.0015, -0.342208, 0, 0.939623},
{94.85, -7.25, -197.01, 0.0015, -0.342208, 0, 0.939623},
{94.6, -7.67, -197.12, 0.0006, -0.354015, 0, 0.93524},
{94.6, -7.67, -197.12, 0.0006, -0.354015, 0, 0.93524},
{94.6, -7.67, -197.12, 0.0006, -0.354015, 0, 0.93524},
{94.33, -6.76, -197.36, 0.009001, -0.36573, 0, 0.930677},
{93.95, -7.48, -197.67, 0.009401, -0.37943, 0, 0.925173},
{93.95, -7.48, -197.67, 0.009401, -0.37943, 0, 0.925173},
{93.48, -6.51, -197.91, 0.018602, -0.395938, 0, 0.918089},
{93.48, -6.51, -197.91, 0.018602, -0.395938, 0, 0.918089},
{93.75, -9.18, -198.25, 0.0047, -0.419231, 0, 0.907867},
{93.75, -9.18, -198.25, 0.0047, -0.419231, 0, 0.907867},
{93.4, -8.07, -199.01, 0.015701, -0.436325, 0, 0.899652},
{93.4, -8.07, -199.01, 0.015701, -0.436325, 0, 0.899652},
{93.1, -9.6, -199.29, 0.014301, -0.455031, 0, 0.890361},
{93.1, -9.6, -199.29, 0.014301, -0.455031, 0, 0.890361},
{92.89, -8.99, -199.82, 0.023101, -0.474128, 0, 0.880153},
{92.89, -8.99, -199.82, 0.023101, -0.474128, 0, 0.880153},
{92.63, -9.44, -200.2, 0.025401, -0.49392, 0, 0.869136},
{92.63, -9.44, -200.2, 0.025401, -0.49392, 0, 0.869136},
{92.29, -9.34, -200.74, 0.0287, -0.512003, 0, 0.858504},
{92.29, -9.34, -200.74, 0.0287, -0.512003, 0, 0.858504},
{92.08, -9.37, -201.04, 0.030101, -0.529723, 0, 0.847637},
{92.08, -9.37, -201.04, 0.030101, -0.529723, 0, 0.847637},
{91.73, -9.29, -201.65, 0.030302, -0.545645, 0, 0.837469},
{91.73, -9.29, -201.65, 0.030302, -0.545645, 0, 0.837469},
{91.78, -9, -201.71, 0.031302, -0.563145, 0, 0.825765},
{91.78, -9, -201.71, 0.031302, -0.563145, 0, 0.825765},
{91.23, -9.24, -202.41, 0.029401, -0.577121, 0, 0.816129},
{91.23, -9.24, -202.41, 0.029401, -0.577121, 0, 0.816129},
{91.31, -9.03, -202.61, 0.028003, -0.593055, 0, 0.804675},
{91.31, -9.03, -202.61, 0.028003, -0.593055, 0, 0.804675},
{91.05, -9.22, -203.23, 0.024501, -0.60752, 0, 0.793926},
{91.05, -9.22, -203.23, 0.024501, -0.60752, 0, 0.793926},
{91.14, -8.79, -203.43, 0.024702, -0.622343, 0, 0.782355},
{91.14, -8.79, -203.43, 0.024702, -0.622343, 0, 0.782355},
{90.69, -9.29, -203.96, 0.022003, -0.63488, 0, 0.772297},
{90.69, -9.29, -203.96, 0.022003, -0.63488, 0, 0.772297},
{90.91, -8.83, -204.29, 0.020201, -0.649818, 0, 0.759821},
{90.91, -8.83, -204.29, 0.020201, -0.649818, 0, 0.759821},
{90.86, -9.63, -205.07, 0.015201, -0.663032, 0, 0.748436},
{90.86, -9.63, -205.07, 0.015201, -0.663032, 0, 0.748436},
{91.02, -7.91, -204.91, 0.0209, -0.676107, 0, 0.736507},
{91.02, -7.91, -204.91, 0.0209, -0.676107, 0, 0.736507},
{90.22, -9.2, -205.44, 0.016601, -0.687445, 0, 0.726047},
{90.22, -9.2, -205.44, 0.016601, -0.687445, 0, 0.726047},
{90.51, -8.84, -205.71, 0.014201, -0.701463, 0, 0.712564},
{90.51, -8.84, -205.71, 0.014201, -0.701463, 0, 0.712564},
{90.23, -9.03, -206.31, 0.0095, -0.714634, 0, 0.699434},
{90.23, -9.03, -206.31, 0.0095, -0.714634, 0, 0.699434},
{90.52, -8.61, -206.55, 0.0058, -0.729044, 0, 0.684442},
{90.52, -8.61, -206.55, 0.0058, -0.729044, 0, 0.684442},
{90, -8.9, -207.04, 0.0038, -0.741237, 0, 0.671233},
{90, -8.9, -207.04, 0.0038, -0.741237, 0, 0.671233},
{90.49, -8.68, -207.25, -0.0004, -0.755154, 0, 0.655547},
{90.49, -8.68, -207.25, -0.0004, -0.755154, 0, 0.655547},
{90.11, -8.78, -207.87, -0.0044, -0.767246, 0, 0.641338},
{90.11, -8.78, -207.87, -0.0044, -0.767246, 0, 0.641338},
{90.52, -8.51, -208.02, -0.008201, -0.779259, 0, 0.626648},
{90.52, -8.51, -208.02, -0.008201, -0.779259, 0, 0.626648},
{90.11, -8.7, -208.12, -0.0088, -0.788141, 0, 0.615432},
{90.11, -8.7, -208.12, -0.0088, -0.788141, 0, 0.615432},
{90.46, -8.52, -208.5, -0.012301, -0.79746, 0, 0.603246},
{90.46, -8.52, -208.5, -0.012301, -0.79746, 0, 0.603246},
{90.15, -8.61, -208.68, -0.014601, -0.805128, 0, 0.592921},
{90.15, -8.61, -208.68, -0.014601, -0.805128, 0, 0.592921},
{90.54, -8.47, -208.84, -0.018102, -0.812294, 0, 0.582967},
{90.54, -8.47, -208.84, -0.018102, -0.812294, 0, 0.582967},
{90.14, -8.5, -208.78, -0.0192, -0.817216, 0, 0.576011},
{90.14, -8.5, -208.78, -0.0192, -0.817216, 0, 0.576011},
{90.63, -8.39, -209.03, -0.023501, -0.822947, 0, 0.567632},
{90.63, -8.39, -209.03, -0.023501, -0.822947, 0, 0.567632},
{90.36, -8.44, -209.01, -0.026602, -0.827765, 0, 0.560444},
{90.36, -8.44, -209.01, -0.026602, -0.827765, 0, 0.560444},
{90.72, -8.34, -209.58, -0.032404, -0.832505, 0, 0.55307},
{90.72, -8.34, -209.58, -0.032404, -0.832505, 0, 0.55307},
{90.72, -8.34, -209.58, -0.032404, -0.832505, 0, 0.55307},
{90.51, -8.23, -209.27, -0.035701, -0.835833, 0, 0.547822},
{90.72, -8.34, -209.74, -0.038705, -0.838407, 0, 0.543669},
{90.72, -8.34, -209.74, -0.038705, -0.838407, 0, 0.543669},
{90.53, -8.28, -209.36, -0.040504, -0.840079, 0, 0.540951},
{90.53, -8.28, -209.36, -0.040504, -0.840079, 0, 0.540951},
{90.66, -8.29, -209.57, -0.044903, -0.840762, 0, 0.53954},
{90.66, -8.29, -209.57, -0.044903, -0.840762, 0, 0.53954},
{90.46, -8.1, -209.2, -0.048902, -0.840036, 0, 0.540323},
{90.46, -8.1, -209.2, -0.048902, -0.840036, 0, 0.540323},
{91.42, -8.66, -210.65, -0.038903, -0.835155, 0, 0.548636},
{91.42, -8.66, -210.65, -0.038903, -0.835155, 0, 0.548636},
{91.07, -8.47, -210.24, -0.042804, -0.836269, 0, 0.546645},
{90.71, -8.31, -209.89, -0.047006, -0.836613, 0, 0.545774},
{90.71, -8.31, -209.89, -0.047006, -0.836613, 0, 0.545774},
{90.35, -8.09, -209.47, -0.052204, -0.836064, 0, 0.546142},
{90.35, -8.09, -209.47, -0.052204, -0.836064, 0, 0.546142},
{90.35, -8.09, -209.47, -0.052204, -0.836064, 0, 0.546142},
{90.02, -7.94, -209.13, -0.058305, -0.834674, 0, 0.547649},
{90.02, -7.94, -209.13, -0.058305, -0.834674, 0, 0.547649},
{89.75, -7.76, -208.87, -0.066005, -0.832659, 0, 0.549839},
{89.75, -7.76, -208.87, -0.066005, -0.832659, 0, 0.549839},
{90.54, -7.59, -209.39, -0.127009, -0.814257, 0, 0.56644},
{90.54, -7.59, -209.39, -0.127009, -0.814257, 0, 0.56644},
{90.67, -6.82, -208.53, -0.148309, -0.808548, 0, 0.569434},
{90.67, -6.82, -208.53, -0.148309, -0.808548, 0, 0.569434},
{90.63, -7.18, -209.1, -0.168912, -0.802358, 0, 0.572442},
{90.63, -7.18, -209.1, -0.168912, -0.802358, 0, 0.572442},
{90.63, -7.18, -209.1, -0.168912, -0.802358, 0, 0.572442},
{90.76, -6.2, -208.26, -0.195111, -0.796544, 0, 0.572232},
{90.73, -6.78, -209.41, -0.219321, -0.790276, 0, 0.572155},
{90.73, -6.78, -209.41, -0.219321, -0.790276, 0, 0.572155},
{91.01, -5.57, -208.34, -0.24632, -0.783164, 0, 0.570947},
{91.01, -5.57, -208.34, -0.24632, -0.783164, 0, 0.570947},
{90.84, -6.5, -209.66, -0.266122, -0.776364, 0, 0.571347},
{90.84, -6.5, -209.66, -0.266122, -0.776364, 0, 0.571347},
{91.18, -4.8, -208.2, -0.29263, -0.767279, 0, 0.570659},
{91.18, -4.8, -208.2, -0.29263, -0.767279, 0, 0.570659},
{91.03, -5.76, -209.32, -0.31093, -0.759974, 0, 0.570756},
{91.03, -5.76, -209.32, -0.31093, -0.759974, 0, 0.570756},
{91.25, -4.48, -208.06, -0.33352, -0.750046, 0, 0.571135},
{91.25, -4.48, -208.06, -0.33352, -0.750046, 0, 0.571135},
{91.2, -5.66, -209.51, -0.347026, -0.742656, 0, 0.572743},
{91.2, -5.66, -209.51, -0.347026, -0.742656, 0, 0.572743},
{91.43, -3.98, -208.03, -0.365542, -0.731585, 0, 0.575467},
{91.43, -3.98, -208.03, -0.365542, -0.731585, 0, 0.575467},
{91.47, -5.18, -209.26, -0.375923, -0.723544, 0, 0.578935},
{91.47, -5.18, -209.26, -0.375923, -0.723544, 0, 0.578935},
{91.57, -3.55, -207.71, -0.389821, -0.711838, 0, 0.584231},
{91.57, -3.55, -207.71, -0.389821, -0.711838, 0, 0.584231},
{91.55, -4.98, -209.03, -0.398032, -0.703357, 0, 0.588948},
{91.55, -4.98, -209.03, -0.398032, -0.703357, 0, 0.588948},
{91.66, -3.1, -207.48, -0.41184, -0.689968, 0, 0.595258},
{91.66, -3.1, -207.48, -0.41184, -0.689968, 0, 0.595258},
{91.82, -4.67, -208.95, -0.419132, -0.681552, 0, 0.599846},
{91.82, -4.67, -208.95, -0.419132, -0.681552, 0, 0.599846},
{91.83, -2.83, -207.1, -0.429724, -0.668738, 0, 0.606734},
{91.83, -2.83, -207.1, -0.429724, -0.668738, 0, 0.606734},
{91.95, -4.14, -208.22, -0.436521, -0.659632, 0, 0.611829},
{91.95, -4.14, -208.22, -0.436521, -0.659632, 0, 0.611829},
{91.73, -2.37, -206.65, -0.444828, -0.64724, 0, 0.619038},
{91.73, -2.37, -206.65, -0.444828, -0.64724, 0, 0.619038},
{92.05, -3.86, -207.97, -0.449334, -0.640048, 0, 0.623247},
{92.05, -3.86, -207.97, -0.449334, -0.640048, 0, 0.623247},
{91.69, -2.05, -206.21, -0.454962, -0.628786, 0, 0.630586},
{91.69, -2.05, -206.21, -0.454962, -0.628786, 0, 0.630586},
{92.38, -3.89, -208.24, -0.459348, -0.622366, 0, 0.633767},
{92.38, -3.89, -208.24, -0.459348, -0.622366, 0, 0.633767},
{92, -1.91, -206.32, -0.461841, -0.612355, 0, 0.641657},
{92, -1.91, -206.32, -0.461841, -0.612355, 0, 0.641657},
{92.35, -3.4, -207.47, -0.463438, -0.60735, 0, 0.645253},
{92.35, -3.4, -207.47, -0.463438, -0.60735, 0, 0.645253},
{91.72, -1.66, -205.72, -0.465058, -0.598975, 0, 0.651882},
{91.72, -1.66, -205.72, -0.465058, -0.598975, 0, 0.651882},
{91.72, -1.66, -205.72, -0.465058, -0.598975, 0, 0.651882},
{92.03, -1.88, -206.09, -0.468942, -0.588552, 0, 0.658559},
{92.03, -1.88, -206.09, -0.468942, -0.588552, 0, 0.658559},
{92.69, -3.2, -207.2, -0.471217, -0.58452, 0, 0.660523},
{92.69, -3.2, -207.2, -0.471217, -0.58452, 0, 0.660523},
{91.9, -1.65, -205.69, -0.472646, -0.575757, 0, 0.667165},
{91.9, -1.65, -205.69, -0.472646, -0.575757, 0, 0.667165},
{93.2, -3.05, -207.09, -0.47623, -0.570836, 0, 0.668843},
{93.2, -3.05, -207.09, -0.47623, -0.570836, 0, 0.668843},
{93.2, -3.05, -207.09, -0.47623, -0.570836, 0, 0.668843},
{92.39, -1.98, -206.05, -0.474626, -0.562931, 0, 0.676638},
{93.1, -2.85, -206.69, -0.471451, -0.55866, 0, 0.682373},
{93.1, -2.85, -206.69, -0.471451, -0.55866, 0, 0.682373},
{93.1, -2.85, -206.69, -0.471451, -0.55866, 0, 0.682373},
{92.92, -2.57, -205.93, -0.462457, -0.545867, 0, 0.698686},
{92.92, -2.57, -205.93, -0.462457, -0.545867, 0, 0.698686},
{92.58, -2.01, -205.56, -0.458237, -0.537943, 0, 0.707557},
{92.58, -2.01, -205.56, -0.458237, -0.537943, 0, 0.707557},
{93.79, -3.07, -206.56, -0.453937, -0.534044, 0, 0.713259},
{93.79, -3.07, -206.56, -0.453937, -0.534044, 0, 0.713259},
{93.35, -2.68, -206, -0.445557, -0.527567, 0, 0.723292},
{93.35, -2.68, -206, -0.445557, -0.527567, 0, 0.723292},
{93.35, -2.68, -206, -0.445557, -0.527567, 0, 0.723292},
{93.42, -2.94, -205.82, -0.428338, -0.517345, 0, 0.740865},
{93.42, -2.94, -205.82, -0.428338, -0.517345, 0, 0.740865},
{93.93, -3.4, -205.78, -0.420034, -0.513641, 0, 0.74816},
{93.93, -3.4, -205.78, -0.420034, -0.513641, 0, 0.74816},
{93.73, -3.66, -205.88, -0.411532, -0.50864, 0, 0.756259},
{93.73, -3.66, -205.88, -0.411532, -0.50864, 0, 0.756259},
{93.94, -3.61, -205.15, -0.401245, -0.506056, 0, 0.763485},
{93.94, -3.61, -205.15, -0.401245, -0.506056, 0, 0.763485},
{93.94, -3.61, -205.15, -0.401245, -0.506056, 0, 0.763485},
{94.37, -4.11, -204.44, -0.376723, -0.485429, 0, 0.788948},
{94.37, -4.11, -204.44, -0.376723, -0.485429, 0, 0.788948},
{95.39, -6.01, -206.38, -0.376248, -0.481461, 0, 0.791601},
{95.39, -6.01, -206.38, -0.376248, -0.481461, 0, 0.791601},
{94.14, -3.89, -203.78, -0.361428, -0.476137, 0, 0.801663},
{94.14, -3.89, -203.78, -0.361428, -0.476137, 0, 0.801663},
{94.76, -5.5, -205.06, -0.361041, -0.470954, 0, 0.804892},
{94.76, -5.5, -205.06, -0.361041, -0.470954, 0, 0.804892},
{93.95, -3.47, -202.94, -0.347244, -0.463359, 0, 0.815304},
{93.95, -3.47, -202.94, -0.347244, -0.463359, 0, 0.815304},
{96.24, -7.06, -205.35, -0.35203, -0.459439, 0, 0.81547},
{96.24, -7.06, -205.35, -0.35203, -0.459439, 0, 0.81547},
{93.94, -3.34, -201.98, -0.328632, -0.447744, 0, 0.831581},
{93.94, -3.34, -201.98, -0.328632, -0.447744, 0, 0.831581},
{96.55, -7.69, -204.58, -0.338831, -0.443841, 0, 0.829577},
{96.55, -7.69, -204.58, -0.338831, -0.443841, 0, 0.829577},
{93.78, -3.09, -201.36, -0.314805, -0.429907, 0, 0.846214},
{93.78, -3.09, -201.36, -0.314805, -0.429907, 0, 0.846214},
{93.97, -3.29, -200.81, -0.300735, -0.414548, 0, 0.8589},
{93.97, -3.29, -200.81, -0.300735, -0.414548, 0, 0.8589},
{93.97, -3.29, -200.81, -0.300735, -0.414548, 0, 0.8589},
{99.3, -10.98, -204.33, -0.326028, -0.420937, 0, 0.846474},
{99.3, -10.98, -204.33, -0.326028, -0.420937, 0, 0.846474},
{94.52, -4.01, -200.67, -0.288925, -0.402834, 0, 0.868474},
{94.52, -4.01, -200.67, -0.288925, -0.402834, 0, 0.868474},
{100.12, -12.19, -203.93, -0.316824, -0.412132, 0, 0.854266},
{100.12, -12.19, -203.93, -0.316824, -0.412132, 0, 0.854266},
{101.43, -14.36, -203.47, -0.306711, -0.408614, 0, 0.85963},
{101.43, -14.36, -203.47, -0.306711, -0.408614, 0, 0.85963},
{101.43, -14.36, -203.47, -0.306711, -0.408614, 0, 0.85963},
{102.21, -15.93, -202.78, -0.29233, -0.406142, 0, 0.86579},
{102.21, -15.93, -202.78, -0.29233, -0.406142, 0, 0.86579},
{97.54, -9.19, -200.43, -0.245927, -0.389243, 0, 0.887699},
{97.54, -9.19, -200.43, -0.245927, -0.389243, 0, 0.887699},
{99.46, -12.82, -200.63, -0.246824, -0.393239, 0, 0.885687},
{99.46, -12.82, -200.63, -0.246824, -0.393239, 0, 0.885687},
{95.63, -7.19, -199.23, -0.202215, -0.377728, 0, 0.903566},
{95.63, -7.19, -199.23, -0.202215, -0.377728, 0, 0.903566},
{97.31, -10.17, -199.4, -0.202706, -0.383012, 0, 0.901228},
{97.31, -10.17, -199.4, -0.202706, -0.383012, 0, 0.901228},
{96.91, -9.55, -199.11, -0.183812, -0.381624, 0, 0.905857},
{96.91, -9.55, -199.11, -0.183812, -0.381624, 0, 0.905857},
{95.61, -7.92, -198.4, -0.155408, -0.376119, 0, 0.913446},
{95.61, -7.92, -198.4, -0.155408, -0.376119, 0, 0.913446},
{95.33, -7.99, -198.33, -0.135013, -0.375135, 0, 0.917085},
{95.33, -7.99, -198.33, -0.135013, -0.375135, 0, 0.917085},
{94.28, -5.67, -197.66, -0.099007, -0.372426, 0, 0.922765},
{94.28, -5.67, -197.66, -0.099007, -0.372426, 0, 0.922765},
{94.43, -7.08, -197.83, -0.084904, -0.375217, 0, 0.923041},
{94.43, -7.08, -197.83, -0.084904, -0.375217, 0, 0.923041},
{93.89, -5.47, -197.44, -0.052104, -0.375726, 0, 0.925265},
{93.89, -5.47, -197.44, -0.052104, -0.375726, 0, 0.925265},
{94.33, -7.49, -197.77, -0.041403, -0.380725, 0, 0.923761},
{94.33, -7.49, -197.77, -0.041403, -0.380725, 0, 0.923761},
{93.63, -5.43, -197.44, -0.0047, -0.380937, 0, 0.924589},
{93.63, -5.43, -197.44, -0.0047, -0.380937, 0, 0.924589},
{93.89, -8.1, -198.02, 0.0016, -0.388021, 0, 0.921649},
{93.89, -8.1, -198.02, 0.0016, -0.388021, 0, 0.921649},
{93.71, -7.17, -197.6, 0.029602, -0.393723, 0, 0.918753},
{93.78, -8.7, -198.37, 0.043905, -0.399241, 0, 0.915794},
{93.78, -8.7, -198.37, 0.043905, -0.399241, 0, 0.915794},
{93.67, -8.4, -198.44, 0.070808, -0.403346, 0, 0.912304},
{93.67, -8.4, -198.44, 0.070808, -0.403346, 0, 0.912304},
{93.58, -10.05, -198.75, 0.087404, -0.409218, 0, 0.908241},
{93.58, -10.05, -198.75, 0.087404, -0.409218, 0, 0.908241},
{93.68, -10.71, -198.82, 0.111209, -0.414835, 0, 0.903075},
{93.68, -10.71, -198.82, 0.111209, -0.414835, 0, 0.903075},
{93.31, -11.29, -199.09, 0.135112, -0.416838, 0, 0.898883},
{93.31, -11.29, -199.09, 0.135112, -0.416838, 0, 0.898883},
{93.37, -11.87, -199.27, 0.158012, -0.420631, 0, 0.893366},
{93.37, -11.87, -199.27, 0.158012, -0.420631, 0, 0.893366},
{93.11, -11.9, -199.57, 0.181609, -0.423421, 0, 0.887543},
{93.11, -11.9, -199.57, 0.181609, -0.423421, 0, 0.887543},
{93.39, -12.58, -199.84, 0.201212, -0.429526, 0, 0.880353},
{93.39, -12.58, -199.84, 0.201212, -0.429526, 0, 0.880353},
{93.39, -12.58, -199.84, 0.201212, -0.429526, 0, 0.880353},
{92.96, -12.41, -200.22, 0.22542, -0.431939, 0, 0.873278},
{93.19, -13.22, -200.43, 0.242822, -0.439341, 0, 0.86488},
{93.19, -13.22, -200.43, 0.242822, -0.439341, 0, 0.86488},
{92.98, -13.02, -200.97, 0.263826, -0.444043, 0, 0.856284},
{92.98, -13.02, -200.97, 0.263826, -0.444043, 0, 0.856284},
{93.09, -13.81, -201.06, 0.280835, -0.451157, 0, 0.847106},
{93.09, -13.81, -201.06, 0.280835, -0.451157, 0, 0.847106},
{93.09, -13.81, -201.06, 0.280835, -0.451157, 0, 0.847106},
{92.88, -13.58, -201.63, 0.302317, -0.456025, 0, 0.837046},
{93.07, -14.44, -201.62, 0.318032, -0.464346, 0, 0.826582},
{93.07, -14.44, -201.62, 0.318032, -0.464346, 0, 0.826582},
{93.07, -14.44, -201.62, 0.318032, -0.464346, 0, 0.826582},
{92.83, -14.02, -202.34, 0.340141, -0.468257, 0, 0.815499},
{92.97, -14.82, -202.44, 0.359338, -0.47295, 0, 0.804484},
{92.97, -14.82, -202.44, 0.359338, -0.47295, 0, 0.804484},
{92.75, -14.39, -203.17, 0.383708, -0.47451, 0, 0.792217},
{92.75, -14.39, -203.17, 0.383708, -0.47451, 0, 0.792217},
{93.14, -15.03, -203.34, 0.404527, -0.478832, 0, 0.779152},
{93.14, -15.03, -203.34, 0.404527, -0.478832, 0, 0.779152},
{92.86, -14.62, -204.02, 0.427743, -0.480549, 0, 0.765577},
{92.86, -14.62, -204.02, 0.427743, -0.480549, 0, 0.765577},
{92.93, -15.17, -204.22, 0.446341, -0.484945, 0, 0.752069},
{92.93, -15.17, -204.22, 0.446341, -0.484945, 0, 0.752069},
{92.8, -14.96, -204.95, 0.46452, -0.490621, 0, 0.737232},
{92.8, -14.96, -204.95, 0.46452, -0.490621, 0, 0.737232},
{93.12, -15.43, -205.19, 0.478536, -0.500938, 0, 0.721155},
{93.12, -15.43, -205.19, 0.478536, -0.500938, 0, 0.721155}
};

static const unsigned int NUMBER_OF_DEGENERATE_TRANSFORMATIONS = 30;
static double pivotCalibrationDegenerateDataSet[NUMBER_OF_DEGENERATE_TRANSFORMATIONS][7] = 
{
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.4, -8.59, -205.45, -0.011301, -0.691185, 0, 0.722589},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.44, -8.63, -205.52, -0.011801, -0.69154, 0, 0.722242},
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.4, -8.59, -205.45, -0.011301, -0.691185, 0, 0.722589},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.44, -8.63, -205.52, -0.011801, -0.69154, 0, 0.722242},
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.39, -8.58, -205.45, -0.011201, -0.691084, 0, 0.722688},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.42, -8.58, -205.46, -0.011301, -0.691135, 0, 0.722637},
{90.4, -8.59, -205.45, -0.011301, -0.691185, 0, 0.722589},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.42, -8.6, -205.47, -0.011501, -0.691236, 0, 0.722538},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.4, -8.6, -205.48, -0.011601, -0.691337, 0, 0.722439},
{90.44, -8.63, -205.52, -0.011801, -0.69154, 0, 0.722242}
};

/** Test the pivot calibration algorithm class as follows:
 *  1. Cause all possible invalid request errors by invoking all RequestXYZ() 
 *     methods before computation.
 *  2. Use a valid data set and give it to the algorithm in one vector, check 
 *     that computation results in expected values.
 *  3. Reset and redo computation after giving the algorithm the transformations
 *     one by one.
 *     Check that we get the same results as in the previous step.
 *  4. Reset and run with degenerate set of transformations, check that the 
 *     algorithm fails. */
int igstkPivotCalibrationAlgorithmTest( int, char * [] )
{
  std::vector< igstk::Transform > transformations;
  igstk::Transform::VectorType translation;
  igstk::Transform::VersorType rotation;
  igstk::Transform currentTransform, computedTransform1, computedTransform2;
  igstk::Transform composedTransform;
  itk::Point< double, 3 > pivotPoint1, pivotPoint2;
  double calibrationRMSE1, calibrationRMSE2;

             //required by IGSTK
  igstk::RealTimeClock::Initialize();

              //the class we are testing
  igstk::PivotCalibrationAlgorithm::Pointer pivotCalibrationAlgorithm = 
    igstk::PivotCalibrationAlgorithm::New();
                 //observers for all events generated by this class
  InvalidRequestErrorEventObserver::Pointer invalidRequestErrorObserver = 
    InvalidRequestErrorEventObserver::New();
  CalibrationEventObserver::Pointer calibrationEventObserver = 
    CalibrationEventObserver::New();
  TransformEventObserver::Pointer transformEventObserver = 
    TransformEventObserver::New();
  PivotPointEventObserver::Pointer pivotPointEventObserver = 
    PivotPointEventObserver::New();
  RMSEEventObserver::Pointer rmseEventObserver = 
    RMSEEventObserver::New();
                  //attach all observers
  pivotCalibrationAlgorithm->AddObserver(igstk::InvalidRequestErrorEvent(), 
                                         invalidRequestErrorObserver);
  pivotCalibrationAlgorithm->AddObserver(igstk::PivotCalibrationAlgorithm::CalibrationSuccessEvent(), 
                                         calibrationEventObserver);
  pivotCalibrationAlgorithm->AddObserver(igstk::PivotCalibrationAlgorithm::CalibrationFailureEvent(), 
                                         calibrationEventObserver);
  pivotCalibrationAlgorithm->AddObserver(igstk::CoordinateSystemTransformToEvent(), 
                                         transformEventObserver);
  pivotCalibrationAlgorithm->AddObserver(igstk::PointEvent(), 
                                         pivotPointEventObserver);
  pivotCalibrationAlgorithm->AddObserver(igstk::DoubleTypeEvent(), 
                                         rmseEventObserver);
  
                //step 1: invoke all methods that cannot be invoked in the 
                //        current object state
  std::cout<<"Next line should report failed calibration:\n\t";
  pivotCalibrationAlgorithm->RequestComputeCalibration();
  if( !calibrationEventObserver->FailureEventOccured() )
    {
    return EXIT_FAILURE;
    }
  calibrationEventObserver->Reset();
  std::cout<<"Next line should report invalid request:\n\t";
  pivotCalibrationAlgorithm->RequestCalibrationTransform();
  if( !invalidRequestErrorObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  invalidRequestErrorObserver->Reset();

  std::cout<<"Next line should report invalid request:\n\t";
  pivotCalibrationAlgorithm->RequestPivotPoint();
  if( !invalidRequestErrorObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  invalidRequestErrorObserver->Reset();

  std::cout<<"Next line should report invalid request:\n\t";
  pivotCalibrationAlgorithm->RequestCalibrationRMSE();
  if( !invalidRequestErrorObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  invalidRequestErrorObserver->Reset();

  std::cout<<"Next line should work:\n\t";
  pivotCalibrationAlgorithm->RequestSetSingularValueThreshold(0.01);
  if( invalidRequestErrorObserver->EventOccured() )
  {
    return EXIT_FAILURE;
  }
  invalidRequestErrorObserver->Reset();

  // code coverage
  pivotCalibrationAlgorithm->Print(std::cout);

             //step 2: use a valid data set and give it in one vector, check 
             //        that computation results in expected values
  for( unsigned int i=0; i<NUMBER_OF_VALID_TRANSFORMATIONS; i++ ) 
    {
    translation[0] = pivotCalibrationValidDataSet[i][0];  
    translation[1] = pivotCalibrationValidDataSet[i][1];  
    translation[2] = pivotCalibrationValidDataSet[i][2];  
            //x,y,z,w
    rotation.Set( pivotCalibrationValidDataSet[i][3], 
                  pivotCalibrationValidDataSet[i][4], 
                  pivotCalibrationValidDataSet[i][5], 
                  pivotCalibrationValidDataSet[i][6] );
                          //minimal possible value is used for error, and 
                          //maximal possible value is used for validity time
    currentTransform.SetTranslationAndRotation( translation, 
                                                rotation, 
                                                itk::NumericTraits<double>::min(), 
                                                itk::NumericTraits<double>::max() );
    transformations.push_back( currentTransform );
    }  
  pivotCalibrationAlgorithm->RequestAddTransforms( transformations );

  std::cout<<"Next line should report successful calibration:\n\t";
  pivotCalibrationAlgorithm->RequestComputeCalibration();
  if( !calibrationEventObserver->SuccessEventOccured() )
    {
    return EXIT_FAILURE;
    }
  calibrationEventObserver->Reset();

  pivotCalibrationAlgorithm->RequestCalibrationTransform();
  if( !transformEventObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  transformEventObserver->Reset();
  igstk::CoordinateSystemTransformToResult transformCarrier1 = 
    transformEventObserver->Get();
  computedTransform1 = transformCarrier1.GetTransform();
  std::cout<<"Next line should show the calibration transformation:\n";
  std::cout<<computedTransform1<<std::endl;

  pivotCalibrationAlgorithm->RequestPivotPoint();
  if( !pivotPointEventObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  pivotPointEventObserver->Reset();
  pivotPoint1 = pivotPointEventObserver->Get();
  std::cout<<"Next line should show the pivot point:\n";
  std::cout<<pivotPoint1<<std::endl;

  pivotCalibrationAlgorithm->RequestCalibrationRMSE();
  if( !rmseEventObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  rmseEventObserver->Reset();
  calibrationRMSE1 = rmseEventObserver->Get();
  std::cout<<"Next line should show the transformation RMSE:\n";
  std::cout<<calibrationRMSE1<<std::endl;

             //step 3: reset and redo computation after giving the algorithm 
             //        the transformations one by one, compare to previous 
             //        result
  pivotCalibrationAlgorithm->RequestResetCalibration();

  std::cout<<"Next line should report failed calibration:\n\t";
  pivotCalibrationAlgorithm->RequestComputeCalibration();
  if( !calibrationEventObserver->FailureEventOccured() )
    {
    return EXIT_FAILURE;
    }
  calibrationEventObserver->Reset();

  std::cout<<"Next line should report invalid request:\n\t";
  pivotCalibrationAlgorithm->RequestCalibrationTransform();
  if( !invalidRequestErrorObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  invalidRequestErrorObserver->Reset();

  std::cout<<"Next line should report invalid request:\n\t";
  pivotCalibrationAlgorithm->RequestPivotPoint();
  if( !invalidRequestErrorObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  invalidRequestErrorObserver->Reset();

  std::cout<<"Next line should report invalid request:\n\t";
  pivotCalibrationAlgorithm->RequestCalibrationRMSE();
  if( !invalidRequestErrorObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  invalidRequestErrorObserver->Reset();

  for( unsigned int i=0; i<NUMBER_OF_VALID_TRANSFORMATIONS; i++ ) 
    {
    translation[0] = pivotCalibrationValidDataSet[i][0];  
    translation[1] = pivotCalibrationValidDataSet[i][1];  
    translation[2] = pivotCalibrationValidDataSet[i][2];  
            //x,y,z,w
    rotation.Set( pivotCalibrationValidDataSet[i][3], 
                  pivotCalibrationValidDataSet[i][4], 
                  pivotCalibrationValidDataSet[i][5], 
                  pivotCalibrationValidDataSet[i][6] );
                          //minimal possible value is used for error, and 
                          //maximal possible value is used for validity time
    currentTransform.SetTranslationAndRotation( translation,
                                                rotation,
                                                itk::NumericTraits<double>::min(),
                                                itk::NumericTraits<double>::max() );
    pivotCalibrationAlgorithm->RequestAddTransform( currentTransform );
    }
  
  std::cout<<"Next line should report successful calibration:\n\t";
  pivotCalibrationAlgorithm->RequestComputeCalibration();
  if( !calibrationEventObserver->SuccessEventOccured() )
    {
    return EXIT_FAILURE;
    }
  calibrationEventObserver->Reset();

  pivotCalibrationAlgorithm->RequestCalibrationTransform();
  if( !transformEventObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }
  transformEventObserver->Reset();
  igstk::CoordinateSystemTransformToResult transformCarrier2 =
    transformEventObserver->Get();
  computedTransform2 = transformCarrier2.GetTransform();
  composedTransform = igstk::Transform::TransformCompose( computedTransform2, 
                                                          computedTransform1.GetInverse() );
  std::cout<<"Next line should show the identity transformation:\n";
  std::cout<<composedTransform<<std::endl;

  pivotCalibrationAlgorithm->RequestPivotPoint();
  if( !pivotPointEventObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }

  pivotPointEventObserver->Reset();
  pivotPoint2 = pivotPointEventObserver->Get();
  std::cout<<"Next line should show (0,0,0) [result of subtracting the current";
  std::cout<<" and previously computed pivot points]:\n";
  std::cout<<pivotPoint1 - pivotPoint2<<std::endl;

  pivotCalibrationAlgorithm->RequestCalibrationRMSE();

  if( !rmseEventObserver->EventOccured() )
    {
    return EXIT_FAILURE;
    }

  rmseEventObserver->Reset();
  calibrationRMSE2 = rmseEventObserver->Get();
  std::cout<<"Next line should show 0 [result of subtracting the current and";
  std::cout<<" previously computed calibration RMSE]:\n";
  std::cout<<calibrationRMSE1 - calibrationRMSE2<<std::endl;

             //step 4: reset and run with degenerate set of transformations
  pivotCalibrationAlgorithm->RequestResetCalibration();

  for( unsigned int i=0; i<NUMBER_OF_DEGENERATE_TRANSFORMATIONS; i++ ) 
    {
    translation[0] = pivotCalibrationDegenerateDataSet[i][0];  
    translation[1] = pivotCalibrationDegenerateDataSet[i][1];  
    translation[2] = pivotCalibrationDegenerateDataSet[i][2];  
            //x,y,z,w
    rotation.Set( pivotCalibrationDegenerateDataSet[i][3], 
                  pivotCalibrationDegenerateDataSet[i][4], 
                  pivotCalibrationDegenerateDataSet[i][5], 
                  pivotCalibrationDegenerateDataSet[i][6] );
                          //minimal possible value is used for error, and 
                          //maximal possible value is used for validity time
    currentTransform.SetTranslationAndRotation( translation,
                                                rotation,
                                                itk::NumericTraits<double>::min(),
                                                itk::NumericTraits<double>::max() );
    pivotCalibrationAlgorithm->RequestAddTransform( currentTransform );
    }

  std::cout<<"Next line should report failed calibration:\n\t";
  pivotCalibrationAlgorithm->RequestComputeCalibration();

  if( !calibrationEventObserver->FailureEventOccured() )
    {
    return EXIT_FAILURE;
    }

  calibrationEventObserver->Reset();

  return EXIT_SUCCESS;
}
